﻿using System;
using System.Collections;
using SkyR1deR;

namespace Modbus
{
    class Slave
    {
        public Boolean[] DiscreteInputContacts;
        public Boolean[] DiscreteOutputCoils;
        public UInt16[] HoldingRegisters;
        public UInt16[] InputRegisters;
        
        public Slave()
        {
            this.DiscreteInputContacts = new Boolean[2000];
            this.DiscreteOutputCoils = new Boolean[2000];
            this.HoldingRegisters = new UInt16[65535];
            this.InputRegisters = new UInt16[65535];           
        }

        public byte[] ComputeRequest(byte[] PDU) 
        {
            switch (PDU[0]) // FunctionCode
            {
                case 1: return ReadCoils(PDU);
                case 2: return ReadDiscreteInputs(PDU);
                case 3: return ReadHoldingRegisters(PDU);
                case 4: return ReadInputRegister(PDU);
                case 5: return WriteSingleCoil(PDU);
                case 6: return WriteSingleRegister(PDU);
                case 15: return WriteMultipleCoils(PDU);
                case 16: return WriteMultipleRegisters(PDU);
                case 22: return MaskWriteRegister(PDU);
                case 23: return ReadWriteMultipleRegisters(PDU);
                default: return null;
            }            
        }

        private byte[] ReadCoils(byte[] PDU)
        {
            UInt16 StartingAddress = Tools.bytesToUInt16(PDU, 1); //0x0000 à 0xFFFF
            UInt16 NbCoils = Tools.bytesToUInt16(PDU, 3); //0x0001 à 0x07D0 (2000)

            int nbBytes = ((NbCoils / 8) + (NbCoils % 8 > 0 ? 1 : 0));

            byte[] Response = new byte[2 + nbBytes];

            Response[0] = 1;
            Response[1] = (byte)nbBytes;

            bool[] Coils = new bool[NbCoils];
            Array.ConstrainedCopy(DiscreteOutputCoils, StartingAddress, Coils, 0, NbCoils);
            new BitArray(Coils).CopyTo(Response, 2);

            return Response;
        }

        private byte[] ReadDiscreteInputs(byte[] PDU)
        {
            UInt16 StartingAddress = Tools.bytesToUInt16(PDU, 1); //0x0000 à 0xFFFF
            UInt16 NbInputs = Tools.bytesToUInt16(PDU, 3); //0x0001 à 0x07D0 (2000)

            int nbBytes = ((NbInputs / 8) + (NbInputs % 8 > 0 ? 1 : 0));

            byte[] Response = new byte[2 + nbBytes];

            Response[0] = 2;
            Response[1] = (byte)nbBytes;

            bool[] Inputs = new bool[NbInputs];
            Array.ConstrainedCopy(DiscreteInputContacts, StartingAddress, Inputs, 0, NbInputs);
            new BitArray(Inputs).CopyTo(Response, 2);

            return Response;
        }

        private byte[] ReadHoldingRegisters(byte[] PDU)
        {
            UInt16 StartingAddress = Tools.bytesToUInt16(PDU, 1); //0x0000 à 0xFFFF
            UInt16 NbRegs = Tools.bytesToUInt16(PDU, 3); //0x0001 à 0x007D (125)

            int nbBytes = NbRegs * 2;

            byte[] Response = new byte[2 + nbBytes];

            Response[0] = 3;            
            Response[1] = (byte)nbBytes;

            int responseOffset = 2;
            for (int i = StartingAddress; i < StartingAddress + NbRegs; i++)
            {
                Response[responseOffset++] = Tools.getByteHI(HoldingRegisters[i]);
                Response[responseOffset++] = Tools.getByteLO(HoldingRegisters[i]);
            }

            return Response;
        }

        private byte[] ReadInputRegister(byte[] PDU)
        {
            UInt16 StartingAddress = Tools.bytesToUInt16(PDU, 1); //0x0000 à 0xFFFF
            UInt16 NbRegs = Tools.bytesToUInt16(PDU, 3); //0x0001 à 0x007D (125)

            int nbBytes = NbRegs * 2;

            byte[] Response = new byte[2 + nbBytes];

            Response[0] = 4;
            Response[1] = (byte)nbBytes;

            int responseOffset = 2;
            for (int i = StartingAddress; i < StartingAddress + NbRegs; i++)
            {
                Response[responseOffset++] = Tools.getByteHI(InputRegisters[i]);
                Response[responseOffset++] = Tools.getByteLO(InputRegisters[i]);
            }

            return Response;
        }

        private byte[] WriteSingleCoil(byte[] PDU)
        {
            UInt16 OutputAddress = Tools.bytesToUInt16(PDU, 1); //0x0000 à 0xFFFF
            UInt16 OutputValue = Tools.bytesToUInt16(PDU, 3); //0x0000 (OFF) ou 0xFF00 (ON)
            
            if (OutputValue > 0) 
            {
                DiscreteOutputCoils[OutputAddress] = true;
            } 
            else 
            {
                DiscreteOutputCoils[OutputAddress] = false;
            }

            byte[] Response = new byte[5];
            Array.ConstrainedCopy(PDU, 0, Response, 0, 5);

            return Response;
        }

        private byte[] WriteSingleRegister(byte[] PDU)
        {
            UInt16 RegisterAddress = Tools.bytesToUInt16(PDU, 1); //0x0000 à 0xFFFF
            UInt16 RegisterValue = Tools.bytesToUInt16(PDU, 3); //0x0000 à 0xFFFF

            HoldingRegisters[RegisterAddress] = RegisterValue;

            byte[] Response = new byte[5];           
            Array.ConstrainedCopy(PDU, 0, Response, 0, 5);

            return Response;
        }

        private byte[] WriteMultipleCoils(byte[] PDU)
        {
            UInt16 StartingAddress = Tools.bytesToUInt16(PDU, 1);  //0x0000 à 0xFFFF
            UInt16 NbOutputs = Tools.bytesToUInt16(PDU, 3); //0x0001 à 0x07B0 (1968)
            UInt16 NbBytes = PDU[5];

            /*int CurrentAddress = StartingAddress;
            int NbRegs = ((NbOutputs / 16) + (NbOutputs % 16 > 0 ? 1 : 0));
            for (int i = 6; i < 6 + NbRegs * 2; i += 2)
            {
                byte[] RegBytes = new byte[2] {request.pdu[i+1],request.pdu[i]};
                BitArray Outputs = new BitArray(RegBytes);                
            }*/

            byte[] Response = new byte[5];
            Array.ConstrainedCopy(PDU, 0, Response, 0, 5);

            return Response;
        }

        private byte[] WriteMultipleRegisters(byte[] PDU)
        {
            UInt16 StartingAddress = Tools.bytesToUInt16(PDU, 1);  //0x0000 à 0xFFFF
            UInt16 NbRegs = Tools.bytesToUInt16(PDU, 3); //0x0001 à 0x007B (123)
            UInt16 NbBytes = PDU[5];

            int pduOffset = 6;
            for (int i = StartingAddress; i < StartingAddress + NbRegs; i++)
            {
                HoldingRegisters[i] = Tools.bytesToUInt16(PDU, pduOffset);
                pduOffset += 2;
            }

            byte[] Response = new byte[5];
            Array.ConstrainedCopy(PDU, 0, Response, 0, 5);

            return Response;
        }

        private byte[] MaskWriteRegister(byte[] PDU)
        {
            UInt16 RegisterAddress = Tools.bytesToUInt16(PDU, 1); //0x0000 à 0xFFFF
            UInt16 And_Mask = Tools.bytesToUInt16(PDU, 3); //0x0000 à 0xFFFF
            UInt16 Or_Mask = Tools.bytesToUInt16(PDU, 5); //0x0000 à 0xFFFF

            HoldingRegisters[RegisterAddress] = (UInt16) ((HoldingRegisters[RegisterAddress] & And_Mask) | (Or_Mask & (~And_Mask)));

            byte[] Response = new byte[7];
            Array.ConstrainedCopy(PDU, 0, Response, 0, 7);

            return Response;
        }

        private byte[] ReadWriteMultipleRegisters(byte[] PDU)
        {
            return PDU;
        }
    }
}
